#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2/LinearMath/Matrix3x3.h"
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include "tf2/utils.h"
#include "nav_msgs/msg/odometry.hpp"
/**
 * @brief 订阅机器人位姿信息
 */
class RobotPoseSubscriber : public rclcpp::Node
{
public:
    RobotPoseSubscriber() : Node("robot_pose_subscriber")
    {
        RCLCPP_INFO(this->get_logger(), "robot_pose_subscriber is running");
        // 订阅 amcl_pose 获取机器人位姿
        pose_subscription_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
            "/amcl_pose", 10, std::bind(&RobotPoseSubscriber::robot_pose_callback, this, std::placeholders::_1));
        // 订阅 odom 获取机器人速度
        odom_subscription_ = this->create_subscription<nav_msgs::msg::Odometry>(
            "/odom", 10, std::bind(&RobotPoseSubscriber::odom_callback, this, std::placeholders::_1));
    }

private:
    // 位置
    rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_subscription_;
    void robot_pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
    {
        auto position = msg->pose.pose.position;
        // 朝向
        auto orientation = msg->pose.pose.orientation;

        RCLCPP_DEBUG(this->get_logger(), "position: x=%f, y=%f,yaw=%f", position.x, position.y,
                    tf2::getYaw(orientation));

        // RCLCPP_INFO(this->get_logger(), "orientation: x=%f, y=%f, z=%f, w=%f", orientation.x, orientation.y, orientation.z, orientation.w);

        // 将四元数转换为欧拉角 方式一：
        // tf2::Quaternion q(orientation.x, orientation.y, orientation.z, orientation.w);
        // tf2::Matrix3x3 m(q);
        // double roll, pitch, yaw;
        // m.getRPY(roll, pitch, yaw);
        // //打印欧拉角
        // RCLCPP_INFO(this->get_logger(), "欧拉角 roll=%f, pitch=%f, yaw=%f", roll, pitch, yaw);
        // 方式二：
        // RCLCPP_INFO(this->get_logger(), "欧拉角 yaw= '%.3f'", tf2::getYaw(orientation));
    }
    // 速度
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_subscription_;
    void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
    {
        // 到达目的地后不再打印

        // 获取线速度和角速度
        double linear_x = msg->twist.twist.linear.x;
        double linear_y = msg->twist.twist.linear.y;
        double angular_z = msg->twist.twist.angular.z;

        // 打印速度信息
        if (linear_x != 0.0 || linear_y != 0.0 || angular_z != 0.0)
        {
            RCLCPP_DEBUG(this->get_logger(), "线速度: x=%f,y=%f, 角速度: %f", linear_x, linear_y, angular_z);
        }
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<RobotPoseSubscriber>());
    rclcpp::shutdown();
    return 0;
}